﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_ObjectIdentification.cs
 * \brief	This file contains the ObjectIdentification function that 
 *  
 * This file contains the ObjectIdentification function that 
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Antonio Matta \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.03
 * \date      2010-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Antonio MATTA                          2009-11-20 \n
 *      First implementation of the M44_ObjectIdentification class.
 *      
 * - Version 0.02:   Gonzalo RODRIGUEZ                      2010-02-15 \n
 *      Change the function to include the correct interfaces: DynamicObjectList and RobotObjectList structure 
 *      
 *  -Version 0.03: Gonzalo RODRIGUEZ                          2010-06-23 \n
 *      Add the constants that control the whole dynamic object detection algorithm
 * 
 *      
/* ######################################################################### */
using System;
/*This is for using lists*/
using System.Collections;
using System.Collections.Generic;
using System.Text;
/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Basis;
using Interfaces.Complex;
/*Using the C Sharp Matrix Library*/
using NMRS.CSML;
/* M44_SharedClasses */
using NMRS.M44_SharedClasses;
/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;


/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_ObjectDetection
     * 
     *  \brief  This is the class of the M44_ObjectIdentification function.
     *
     * 
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_ObjectIdentification
    {
        /* ------------------------------------------------------------------------- */
        /**	\fn public static cl_DynamicObjects ObjectIdentification( cl_DynamicObjects DynamicObjectsList,
                                                                      int[,] DynamicObjectsAct,
                                                                      float SimulationTime
                                                                    )
         * 
         *  \brief This function finds pairs the objects find in the current iteration with the global list 
         *          of object from this robot
         *     
         *  \param[in] RobotObjectList str_RobotObjectList_act  current list of object and properties
         *  \param[in] RobotObjectList str_RobotObjectList      global list of objects and properties
         *  \param[in] float ActTime                            Simulation time
         *  
         *  \param[out] RobotObjectList str_RobotObjectList
         *
         */
        /* ------------------------------------------------------------------------- */
        public static RobotObjectList ObjectIdentification(RobotObjectList str_RobotObjectList_act,
                                                           RobotObjectList str_RobotObjectList,
                                                           float ActTime,
                                                            char[] RobotID, ushort maxIDrobot)
        {
            int n_list = -1;
            int n_act = -1;
            int n_add = 0;
            double[,] pred_pos;
            double[,] act_pos;
            float[,] pairs;
            float[,] obj_pairs;
            bool first = true;
            float distance;
            int i_aux = 0;
            int max_id = 0;
            float distance_tol = DynObj_Const.f_object_distance_tol; 
            float t_min_detect = DynObj_Const.t_min_detect; 
            float dist_min = 0;
            double[] pos1 = new double[2], pos0 = new double[2];
            double vx, vy;
            int[] eliminate;
            int n_eliminate;
            float inc_time = 0;
            int[] obj_act_added;
            float auxTime;
            cl_DynamicObjects DynObjects_aux = new cl_DynamicObjects();
            //*Identify objects from DynamicObjectsList.List 
            // * with the ones defined in DynamicObjectsAct

            //Calculate the number of elements in the dynamic object list and in the actual object list
            n_list = str_RobotObjectList.DynamicObjects.List.Length;
            n_act = str_RobotObjectList_act.DynamicObjects.List.Length;
            if (n_list == 0 && n_act>0)
            {
                str_RobotObjectList.DynamicObjects = new cl_DynamicObjects((ushort)n_act);
                str_RobotObjectList.DynamicObjects = str_RobotObjectList_act.DynamicObjects;
                    
            }
            else if (n_act > 0 && n_list > 0)
            {
                n_eliminate = 0;

                pred_pos = new double[n_list, 2];
                act_pos = new double[n_act, 2];
                pairs = new float[n_list * n_act, 3]; //ind_pred, ind_act,distance //all the possible pairs
                obj_pairs = new float[Math.Min(n_list, n_act), 3]; //ind_pred,ind_act,distance //selected pairs

                obj_act_added = new int[n_act];
                //calculate the distance between the pairs
                for (int i = 0; i < n_list; i++)
                {
                    pos0[0] = str_RobotObjectList.DynamicObjects.List[i].Position.d_x;
                    pos0[1] = str_RobotObjectList.DynamicObjects.List[i].Position.d_y;
                    vx = str_RobotObjectList.DynamicObjects.List[i].Velocity.d_xVel;
                    vy = str_RobotObjectList.DynamicObjects.List[i].Velocity.d_yVel;
                    auxTime = str_RobotObjectList.DynamicObjects.List[i].f_DetectionTime;
                    pred_pos[i, 0] = pos0[0] + vx * (ActTime - auxTime);
                    pred_pos[i, 1] = pos0[1] + vy * (ActTime - auxTime);
                    for (int j = 0; j < n_act; j++)
                    {
                        if (first)
                        {
                            obj_act_added[j] = 0; // all the objects marked as not added. later mark the one added with 1
                            act_pos[j, 0] = (float)str_RobotObjectList_act.DynamicObjects.List[j].Position.d_x;
                            act_pos[j, 1] = (float)str_RobotObjectList_act.DynamicObjects.List[j].Position.d_y;
                        }
                        distance = (float)Math.Sqrt((double)(Math.Pow((double)(act_pos[j, 0] - pred_pos[i, 0]), 2) +
                                                             Math.Pow((double)(act_pos[j, 1] - pred_pos[i, 1]), 2)));
                        pairs[j + i * n_act, 0] = i;
                        pairs[j + i * n_act, 1] = j;
                        pairs[j + i * n_act, 2] = distance;

                    }
                    first = false;
                }
                //Order and select pairs
                dist_min = 100000;
                while (i_aux < Math.Min(n_list, n_act))
                {
                    for (int i = 0; i < (n_act * n_list); i++)
                    {
                        if (pairs[i, 2] < dist_min && pairs[i,2]>0)
                        {
                            obj_pairs[i_aux, 0] = pairs[i, 0];
                            obj_pairs[i_aux, 1] = pairs[i, 1];
                            obj_pairs[i_aux, 2] = pairs[i, 2];
                            dist_min = pairs[i, 2];
                        }
                    }
                    for (int i = 0; i < (n_act * n_list); i++)
                    {
                        if (pairs[i, 0] == obj_pairs[i_aux, 0])
                            pairs[i, 2] = -1;
                        if (pairs[i, 1] == obj_pairs[i_aux, 1])
                            pairs[i, 2] = -1;
                    }
                    dist_min = 100000;
                    i_aux++;
                    
                }
                //Check each pair to see if distance its acceptable and to predict new position

                for (int i = 0; i < Math.Min(n_act, n_list); i++)
                {
                    if (obj_pairs[i, 2] < distance_tol)
                    {
                        obj_act_added[(int)obj_pairs[i, 1]] = 1;
                        pos1[0] = str_RobotObjectList_act.DynamicObjects.List[(int)obj_pairs[i, 1]].Position.d_x;
                        pos1[1] = str_RobotObjectList_act.DynamicObjects.List[(int)obj_pairs[i, 1]].Position.d_y;
                        pos0[0] = str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].Position.d_x;
                        pos0[1] = str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].Position.d_y;

                        auxTime = str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].f_DetectionTime;
                        //Time Spent in seconds
                        inc_time = (ActTime - auxTime);

                        vx = (pos1[0] - pos0[0]) / inc_time;
                        vy = (pos1[1] - pos0[1]) / inc_time;

                        str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].Position.d_x = pos1[0];
                        str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].Position.d_y = pos1[1];
                        str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].Velocity.d_xVel = vx;
                        str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].Velocity.d_yVel = vy;

                        str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].f_DetectionTime = ActTime;

                        str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].PhysCharac.f_PhysSize = str_RobotObjectList_act.DynamicObjects.List[(int)obj_pairs[i, 1]].PhysCharac.f_PhysSize;
                        max_id = Math.Max(max_id, (int)str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].us_Identification);

                    }
                }
                //Remove objects not seen or actualize position with last known velocity
                eliminate = new int[n_list];
                n_eliminate = 0;
                for (int i = 0; i < n_list; i++)
                {
                    eliminate[i] = 0;
                    if (str_RobotObjectList.DynamicObjects.List[i].f_DetectionTime != ActTime)
                    {
                        auxTime = str_RobotObjectList.DynamicObjects.List[i].f_DetectionTime;
                        if (auxTime + t_min_detect < ActTime)
                        {
                            //eliminate this object
                            eliminate[i] = 1;
                            n_eliminate++;
                        }
                    }
                }
                //Eliminate the objects
                //Create auxiliary array of structures to save the objects that are not going to be eliminated
                i_aux = 0;
                for (int i = 0; i < n_act; i++)
                {
                    if (obj_act_added[i] == 0)
                        n_add++;
                }
                DynObjects_aux = new cl_DynamicObjects((ushort)(n_list - n_eliminate + n_add));
                for (int i = 0; i < n_list; i++)
                {
                    if (eliminate[i] == 0)
                    {
                        DynObjects_aux.List[i_aux] = str_RobotObjectList.DynamicObjects.List[i_aux];
                        i_aux++;
                    }
                }
                max_id = Math.Max(max_id, maxIDrobot);
                //Add the objects that have not been paired
                for (int i = 0; i < n_act; i++)
                {
                    if (obj_act_added[i] == 0)
                    {
                        DynObjects_aux.List[i_aux] = new Interfaces.Complex.cl_DynamicObjectList();
                        DynObjects_aux.List[i_aux].Position.d_x = str_RobotObjectList_act.DynamicObjects.List[i].Position.d_x;
                        DynObjects_aux.List[i_aux].Position.d_y = str_RobotObjectList_act.DynamicObjects.List[i].Position.d_y;
                        DynObjects_aux.List[i_aux].f_DetectionTime = ActTime;
                        DynObjects_aux.List[i_aux].Velocity.d_xVel = 0;
                        DynObjects_aux.List[i_aux].Velocity.d_yVel = 0;
                        DynObjects_aux.List[i_aux].PhysCharac.f_PhysSize = str_RobotObjectList_act.DynamicObjects.List[i].PhysCharac.f_PhysSize;
                        DynObjects_aux.List[i_aux].us_Identification = (ushort)(max_id + 1);
                        max_id++;
                        i_aux++;
                    }
                }
                str_RobotObjectList.DynamicObjects = DynObjects_aux;
                str_RobotObjectList.maxID = (ushort)max_id;

            }
            return str_RobotObjectList;
        }

        
    }
}
